#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/conversions.h>
#include <depth_image_proc/depth_traits.h>
#include <sensor_msgs/Image.h>
#include "geometry_msgs/Twist.h"
#include "tf/tf.h"
#include <tf/transform_listener.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/common.h>
#include <pcl/conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/point_types_conversion.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>

class ObstacleDetector {
public:
    ObstacleDetector(ros::NodeHandle &nh) {
        // 订阅点云数据
        pointcloud_sub_ = nh.subscribe("/camera/depth/points", 10, &ObstacleDetector::pointcloudCallback, this);

        // 发布机器人停止运行的消息
        cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

        tf_listener_ = new tf::TransformListener();
        // 获取机器人到点云坐标系的变换
        tf_listener_->waitForTransform("base_link", "camera_depth_optical_frame", ros::Time(0), ros::Duration(3.0));
        tf_listener_->lookupTransform("base_link", "camera_depth_optical_frame", ros::Time(0), tf_transform_);
    }

    ~ObstacleDetector() {}

    void pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg) {
        // 将点云数据转换到机器人坐标系上
        pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
        pcl::fromROSMsg(*msg, pcl_cloud);
        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
        pcl_ros::transformPointCloud(pcl_cloud, *pcl_cloud_transformed, tf_transform_);

        // 提取点云中高于地面的元素
        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(pcl_cloud_transformed);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(0.0, 0.5); // 假设地面高度为0.5米
        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_ground_removed(new pcl::PointCloud<pcl::PointXYZ>);
        pass.filter(*pcl_cloud_ground_removed);

        // 统计高于地面的点的数量
        int num_obstacles = 0;
        for (size_t i = 0; i < pcl_cloud_ground_removed->size(); ++i) {
            if (pcl_cloud_ground_removed->at(i).z > 0.1) // 假设障碍物高度大于0.1米
            {
                num_obstacles++;
            }
        }

        // 如果障碍物数量超过一定数量，则停止机器人运行if (num_obstacles > 10) // 假设障碍物数量超过10个
        {
            ROS_WARN("Obstacle detected!");
            geometry_msgs::Twist cmd_vel;
            cmd_vel.linear.x = 0.0;
            cmd_vel.angular.z = 0.0;
            cmd_vel_pub_.publish(cmd_vel);
        }
    }

private:
    ros::Subscriber pointcloud_sub_;
    ros::Publisher cmd_vel_pub_;

    tf::TransformListener *tf_listener_;
    tf::StampedTransform tf_transform_;
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "cre_obstacle_detector");
    ros::NodeHandle nh;

//    tf2_ros::Buffer buffer(ros::Duration(10));
//    tf2_ros::TransformListener tf(buffer);
    ObstacleDetector detector(nh);

    ros::spin();

    return 0;
}